#include "modbuspollworker.h"
#include <QDebug>

ModbusPollWorker::ModbusPollWorker(QObject *parent) : QObject(parent), ctx(NULL)
{

}

ModbusPollWorker::~ModbusPollWorker()
{

}

void ModbusPollWorker::open(QString portName, int baudrate, char parity, int databits, int stopbits)
{
    if(ctx != NULL)
        return;
    ctx = modbus_new_rtu(portName.toStdString().c_str(), baudrate, parity, databits, stopbits);
    if(modbus_connect(ctx) < 0)
    {
        modbus_close(ctx);
        modbus_free(ctx);
        ctx = NULL;
        emit opened(-1);
        return;
    }
    modbus_set_response_timeout(ctx, 0, 300000);
    emit opened(0);
}

void ModbusPollWorker::close()
{
    if(ctx == NULL)
        return;
    modbus_close(ctx);
    modbus_free(ctx);
    ctx = NULL;
    emit closed(0);
}

void ModbusPollWorker::modbusRtuMasterRead(quint8 slaveAddress, quint8 functionCode, quint16 startAddress, quint32 numToRead)
{
    QByteArray data;
    uint16_t value[2000];

    if(ctx == NULL)
    {
        goto __failed;
    }

    if(modbus_get_slave(ctx) != slaveAddress)
    {
        modbus_set_slave(ctx, slaveAddress);
    }

    switch(functionCode)
    {

    case 0X01:  // 读线圈
    {
        int offset    = startAddress;
        int totalRead = numToRead;

        while(totalRead != 0)
        {
            int maxToRead = totalRead >= MODBUS_MAX_READ_BITS ? MODBUS_MAX_READ_BITS : totalRead;
            if(modbus_read_bits(ctx, offset, maxToRead, (uint8_t *)value) < 0)
            {
                goto __failed;
            }
            data      += QByteArray((const char *)value, maxToRead);
            offset    += maxToRead;
            totalRead -= maxToRead;
        }

        break;
    }

    case 0X02:  // 读离散量输入
    {
        int offset    = startAddress;
        int totalRead = numToRead;

        while(totalRead != 0)
        {
            int maxToRead = totalRead >= MODBUS_MAX_READ_BITS ? MODBUS_MAX_READ_BITS : totalRead;
            if(modbus_read_input_bits(ctx, offset, maxToRead, (uint8_t *)value) < 0)
            {
                goto __failed;
            }
            data      += QByteArray((const char *)value, maxToRead);
            offset    += maxToRead;
            totalRead -= maxToRead;
        }

        break;
    }

    case 0X03:  // 读保持寄存器
    {
        int offset    = startAddress;
        int totalRead = numToRead;

        while(totalRead != 0)
        {
            int maxToRead = totalRead >= MODBUS_MAX_READ_REGISTERS ? MODBUS_MAX_READ_REGISTERS : totalRead;
            if(modbus_read_registers(ctx, offset, maxToRead, value) < 0)
            {
                goto __failed;
            }
            data      += QByteArray((const char *)value, maxToRead * 2);
            offset    += maxToRead;
            totalRead -= maxToRead;
        }

        break;
    }

    case 0X04:  // 读输入寄存器
    {
        int offset    = startAddress;
        int totalRead = numToRead;

        while(totalRead != 0)
        {
            int maxToRead = totalRead >= MODBUS_MAX_READ_REGISTERS ? MODBUS_MAX_READ_REGISTERS : totalRead;
            if(modbus_read_input_registers(ctx, offset, maxToRead, value) < 0)
            {
                goto __failed;
            }
            data      += QByteArray((const char *)value, maxToRead * 2);
            offset    += maxToRead;
            totalRead -= maxToRead;
        }

        break;
    }

    default:
        break;
    }

    goto __exit;

__failed:
    data.clear();

__exit:
    emit this->response(slaveAddress, functionCode, startAddress, numToRead, data);
}

void ModbusPollWorker::modbusRtuMasterWrite(quint8 slaveAddress, quint8 functionCode, quint16 startAddress, quint32 numToWrite, QByteArray data)
{
    if(ctx == NULL)
    {
        goto __exit;
    }

    if(modbus_get_slave(ctx) != slaveAddress)
    {
        modbus_set_slave(ctx, slaveAddress);
    }

    switch(functionCode)
    {

    case 0X05:  // 写单个线圈
    {
        quint16 value = data[0];
        if(modbus_write_bit(ctx, startAddress, value) < 0)
        {
            goto __failed;
        }
        break;
    }

    case 0X06:  // 写单个寄存器
    {
        quint16 value = ((quint8)data[1] << 8) | (quint8)data[0];
        if(modbus_write_register(ctx, startAddress, value) < 0)
        {
            goto __failed;
        }
        break;
    }

    case 0X0F:  // 写多个线圈
    {
        const uint8_t* pCoilsValue = (const uint8_t *)data.data();
        quint16 offset = startAddress;
        while(numToWrite != 0)
        {
            int maxToWrite = numToWrite >= MODBUS_MAX_WRITE_BITS ? MODBUS_MAX_WRITE_BITS : numToWrite;
            if(modbus_write_bits(ctx, offset, maxToWrite, pCoilsValue) < 0)
            {
                goto __failed;
            }
            numToWrite  -= maxToWrite;
            pCoilsValue += maxToWrite;
            offset      += maxToWrite;
        }
        break;
    }

    case 0X10:  // 写多个寄存器
    {
        const uint16_t* pRegValue = (const uint16_t* )data.data();
        quint16 offset = startAddress;
        while(numToWrite != 0)
        {
            int maxToWrite = numToWrite >= MODBUS_MAX_WRITE_REGISTERS ? MODBUS_MAX_WRITE_REGISTERS : numToWrite;                     // 一次最多写入123个寄存器位
            if(modbus_write_registers(ctx, offset, maxToWrite, pRegValue) < 0)
            {
                goto __failed;
            }
            numToWrite -= maxToWrite;
            pRegValue  += maxToWrite;
            offset     += maxToWrite;
        }
        break;
    }

    default:break;
    }

    goto __exit;

__failed:
    data.clear();

__exit:
    emit this->response(slaveAddress, functionCode, startAddress, numToWrite, data);
}
